/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj;

/**
 *
 * @author matt
 */
public class RateGyro extends Gyro {

    // numner of iterations to average over for rate calibration
    private static final int m_kCalibrationCount = 100;
    // voultage output of gyro when not turning
    private double m_rateCenterVoltage = 0;

    private boolean m_useRateForPID = true;
    
    public RateGyro(int slot, int channel) {
        super(slot, channel);
    }

    public RateGyro(int channel) {
        super(channel);
        calibrateRate();
    }

    /**
     * Return the rate of angular change reported by the gyro
     *
     * @return the rate of change of the gyron angle in degrees/second.
     */
    public double getAngleRate() {

        // Need to look at if getVoltage or getAverageVoltage is the right function
        // since getAverage has a lower update rate
        return (m_analog.getAverageVoltage() - m_rateCenterVoltage) / m_voltsPerDegreePerSecond;
    }

    /**
     * Reset the gyro and recenter the rate
     *
     * Resets the base gyro class and recalibrates the center point for the gyro
     * angular rate
     */
    public void reset() {
        super.reset();
        calibrateRate();
    }

    /**
     * calibrateRate identifies the zero point for the gyro angular rate 
     *   so it can be used to center the gyro results
     */
    private void calibrateRate() {
        // assume robot not moving and find the true center value
        double m_rateCenterVoltageAccumulation = 0;
        for (int count = 0; count < m_kCalibrationCount; count++) {
            m_rateCenterVoltageAccumulation += m_analog.getAverageVoltage();
        }
        m_rateCenterVoltage = m_rateCenterVoltageAccumulation / m_kCalibrationCount;
    }

    /**
     * 
     * @param useAngle is true to use the Absolute Angle with PID controllers, 
     *                    false uses the Angle Rate of change
     *                 
     */
    public void useAngleForPID(boolean useAngle) {
        m_useRateForPID = !useAngle;
    }
       
    /**
     * Get the angle or rate of angle chnage of the gyro for use with PIDControllers
     * @return the current angle or rate of angle change according to the gyro
     */
 
    public double pidGet() {
        if (m_useRateForPID)
        {
            // if using Rate for PID use getAngleRate()
            return getAngleRate();
        }
        else
        {
            // otherwise use the base class pidGet()
            return super.pidGet();
        }             
    }
}
